import pygame
import magnet_plate_robot as mrobot
import time
import threading

CLOCK = pygame.time.Clock()
FPS   = 60
ROBOT:mrobot.Magnet_Plate_Robot = None
SIZE  = [480, 240]
SPD   = [0., 0.]
POS   = [0., 0.]

_thred_run = True

def screen_print(suf:pygame.Surface, texts:str, 
                font:pygame.font.Font, inity=0):
    """使用该函数在屏幕上打印文字"""
    y = inity
    for text in texts.split("\n"):
        text_surface = font.render(text, True, (0, 0, 0))
        suf.blit(text_surface, (0, y))
        y += font.get_height()


def render(surf:pygame.Surface, font):
    surf.fill("white")
    screen_print(surf, 
    f"Speed of robot: [{SPD[0]}, {SPD[1]}]\nPos of robot: [{POS[0]:.2f}, {POS[1]:.2f}]",
    font)
    pygame.display.flip()
    CLOCK.tick(FPS)



def event_handler():
    global SPD, _thred_run
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            _thred_run = False
            exit()
        elif event.type == pygame.KEYDOWN:
            if event.key == pygame.K_UP:
                SPD[0] += 100
            if event.key == pygame.K_DOWN:
                SPD[0] -= 100
            if event.key == pygame.K_LEFT:
                SPD[1] += 100
            if event.key == pygame.K_RIGHT:
                SPD[1] -= 100
        elif event.type == pygame.KEYUP:
            if event.key == pygame.K_UP:
                SPD[0] -= 100
            if event.key == pygame.K_DOWN:
                SPD[0] += 100
            if event.key == pygame.K_LEFT:
                SPD[1] -= 100
            if event.key == pygame.K_RIGHT:
                SPD[1] += 100

def pos_getter(interval=1):
    global POS, ROBOT
    while _thred_run:
        POS[0], POS[1] = ROBOT.get_position()
        time.sleep(interval)

def main():
    global ROBOT
    ser_lock = threading.Lock()
    try:
        ROBOT = mrobot.Magnet_Plate_Robot(lock=ser_lock)
    except Exception as e:
        input(f"机器人启动失败, 可能是未检测到串口连接.错误信息：\n{e}")
        exit()
    
    pygame.init()
    pygame.font.init()
    font = pygame.font.SysFont(name="arial", size=35)
    display = pygame.display.set_mode(SIZE, pygame.RESIZABLE)
    threading.Thread(target=pos_getter).start()
    pygame.display.set_caption(f"robot at {ROBOT.ser.port}")
    while True:
        event_handler()
        render(display, font)
        ROBOT.set_speed(*SPD)

if __name__ == "__main__":
    main()